package packageLego;

import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.Motor;
import lejos.nxt.SensorPort;
import lejos.nxt.addon.CompassSensor;
import lejos.robotics.navigation.DifferentialPilot;

public class Boussole {

	static CompassSensor boussole= new CompassSensor(SensorPort.S2);
	static float wheelDiameter = 2.2f;
	
	static float width	= 4.7f;
	
	static DifferentialPilot moteurs =new DifferentialPilot(wheelDiameter, width, Motor.A, Motor.B);
	
	public static void tourneDroite(float angle)
	{
		/*Motor.B.setSpeed(60);
		Motor.A.setSpeed(60);*/
		moteurs.setTravelSpeed(2);
		
		boussole.resetCartesianZero();
		float positionDept= boussole.getDegrees();

		float positionArrivee;
		if(positionDept<=(359-angle))
		{
			positionArrivee=positionDept+angle;
			while(boussole.getDegrees()<positionArrivee){
				moteurs.rotateRight();
				
				/*Motor.A.forward();
				Motor.B.backward();*/
				LCD.drawString("Boussole :"+boussole.getDegrees(),0,0 );
				LCD.drawString("arrivee:"+positionArrivee,1,1 );
				LCD.refresh();
				}
		}
		else{
			positionArrivee=angle-(359-positionDept);
			while(boussole.getDegrees()!=positionArrivee){
				moteurs.rotateRight();
				/*Motor.A.forward();
				Motor.B.backward();*/
				
				LCD.drawString("Boussole :"+boussole.getDegrees(),0,0 );
				LCD.drawString("arrivee:"+positionArrivee,1,1 );
				LCD.refresh();
				}
			
		}
		moteurs.forward();
		moteurs.setTravelSpeed(5);
		/*Motor.B.forward();
		Motor.A.forward();
		Motor.B.setSpeed(250);
		Motor.A.setSpeed(250);*/
		
	}
	
	public static void tourneGauche(float angle)
	{
		/*Motor.B.setSpeed(60);
		Motor.A.setSpeed(60);*/

		moteurs.setTravelSpeed(2);
		
		
		boussole.resetCartesianZero();
		float positionDept= boussole.getDegrees();

		float positionArrivee;
		if(positionDept>=angle)
		{
			positionArrivee=positionDept-angle;
			while(boussole.getDegrees()>positionArrivee){
				/*
				Motor.B.forward();
				Motor.A.backward();*/
				moteurs.rotateLeft();
				LCD.drawString("Boussole :"+boussole.getDegrees(),0,0 );
				LCD.drawString("cartesien:"+boussole.getDegreesCartesian(),1,1 );
				LCD.refresh();
				}
		}
		else{
			positionArrivee=359-(angle-positionDept);
			while(boussole.getDegrees()!=positionArrivee){/*
				Motor.B.forward();
				Motor.A.backward();*/
				moteurs.rotateLeft();
				}
		}

		moteurs.forward();
		moteurs.setTravelSpeed(5);
		
		/*Motor.B.forward();
		Motor.A.forward();
		Motor.B.setSpeed(250);
		Motor.A.setSpeed(250);*/
		
	}
	
	
	/*static CompassSensor boussole= new CompassSensor(SensorPort.S2);
	
	public static void main(String[] args) throws InterruptedException {
	boussole.resetCartesianZero();
		while(!Button.ESCAPE.isPressed())
			{
			LCD.drawString("Boussole :"+boussole.getDegrees(),0,0 );
			LCD.drawString("Boussole :"+boussole.getDegreesCartesian(),2,2 );
			LCD.refresh();
			}
	}*/
}
